package com.acheli.utils.rotate;


import net.minecraft.world.phys.Vec3;
import org.joml.Quaternionf;
import org.joml.Vector3f;

public class TorqueCalculator {
    // 新增：局部坐标系到世界坐标系的转换方法
    public static Vector3f calculateLocalRotationDirection(
            Vec3 localForcePoint,
            Vec3 pivotPoint,
            Vec3 localForceVector,
            Quaternionf rotation
    ) {
        // 将局部坐标转换为世界坐标
        Vector3f worldForcePoint = transformLocalToWorld(localForcePoint, pivotPoint, rotation);
        Vector3f worldForce = transformLocalVectorToWorld(localForceVector, rotation);

        return calculateRotationDirection(
                new Vec3(worldForcePoint),
                pivotPoint,
                new Vec3(worldForce)
        );
    }

    // 原方法保持兼容性
    public static Vector3f calculateRotationDirection(
            Vec3 forcePoint,
            Vec3 pivotPoint,
            Vec3 forceVector
    ) {
        Vector3f leverArm = forcePoint.toVector3f().sub(pivotPoint.toVector3f());
        Vector3f torque = new Vector3f(leverArm).cross(forceVector.toVector3f());
        return torque;
    }

    public static Vec3 fromVector3f(Vector3f vector3f) {
        return new Vec3(vector3f.x, vector3f.y, vector3f.z);
    }
    public static class CoordinateTransform {
        /**
         * 世界坐标系 → 局部坐标系
         * @param worldVector 世界方向向量
         * @param rotation 物体的当前旋转四元数
         * @return 局部坐标系中的方向向量
         */
        public static Vector3f worldToLocal(Vector3f worldVector, Quaternionf rotation) {
            // 创建旋转四元数的共轭（逆旋转）
            Quaternionf inverseRot = new Quaternionf(rotation).conjugate();

            // 应用逆旋转到世界向量
            Vector3f localVector = new Vector3f(worldVector);
            inverseRot.transform(localVector);

            return localVector;
        }

        /**
         * 局部坐标系 → 世界坐标系
         * @param localVector 局部方向向量
         * @param rotation 物体的当前旋转四元数
         * @return 世界坐标系中的方向向量
         */
        public static Vector3f localToWorld(Vector3f localVector, Quaternionf rotation) {
            Vector3f worldVector = new Vector3f(localVector);
            rotation.transform(worldVector);
            return worldVector;
        }
    }
    // 新增：连续角度计算
    public static class ContinuousRotation {
        private float lastYawRad;
        private float accumulatedYaw;

        public float update(Quaternionf rotation) {
            Vector3f euler = new Vector3f();
            rotation.getEulerAnglesYXZ(euler);

            // 角度差值计算
            float delta = euler.y - lastYawRad;
            delta = wrapAngle(delta);

            accumulatedYaw += delta;
            lastYawRad = euler.y;

            return accumulatedYaw;
        }

        private float wrapAngle(float radian) {
            return radian - (float)(Math.PI * 2) * Math.round(radian / (float)(Math.PI * 2));
        }
    }

    // 新增：坐标转换工具方法
    private static Vector3f transformLocalToWorld(Vec3 localPoint, Vec3 pivot, Quaternionf rotation) {
        Vector3f offset = localPoint.toVector3f();
        rotation.transform(offset);
        return offset.add(pivot.toVector3f());
    }

    private static Vector3f transformLocalVectorToWorld(Vec3 localVector, Quaternionf rotation) {
        Vector3f vec = localVector.toVector3f();
        rotation.transform(vec);
        return vec;
    }

    // 改进后的描述方法
    public static String getRotationDescription(Vector3f torque) {
        float magnitude = torque.length();
        if (magnitude < 1e-5f) return "No rotation";

        Vector3f direction = new Vector3f(torque).normalize();
        return String.format(
                "Torque: %.2f N·m around [%.2f, %.2f, %.2f]",
                magnitude,
                direction.x, direction.y, direction.z
        );
    }
}